
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <base/utils/utils.h>
#include <hardware/Joystick/HAL_Joystick.h>
#include <hardware/Joystick/HAL_JSCombo.h>

using namespace std;
using namespace pi;



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

int Test_Joystick(CParamArray *pa)
{
    int                 dev = 0;

    HAL_JoyStick        js;
    JS_Val              jsv;

    // open device
    dev = svar.GetInt("dev", dev);
    if( 0 != js.open(dev) ) return -1;

    printf("devName     = %s\n", js.m_devName);
    printf("devVersion  = %d\n", js.m_devVersion);
    printf("numAxes     = %d\n", js.m_numAxes);
    printf("numBtns     = %d\n", js.m_numBtns);

    // loop and read joystick axis & button values
    while(1) {
        js.read(&jsv);

        printf("JS Axes (%2d) = ", jsv.dataUpdated);
        for(int i=0; i<16; i++) printf("%6.2f ", jsv.AXIS[i]);
        printf("\n");

        printf("JS Btns (%2d) = ", jsv.dataUpdated);
        for(int i=0; i<16; i++) printf("%6.2f ", jsv.BUTTON[i]);
        printf("\n");

        printf("\n");

        tm_sleep(50);
    }

    return 0;
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

int Test_JSCombo(CParamArray *pa)
{
    HAL_JSComobo        js;
    JS_Val              jsv;

    // load joystick configure
    string confFN = svar.GetString("confFN", "Joystick.cfg");
    svar.ParseFile(confFN);

    // get joystick configure
    string jsDev = svar.GetString("jsDev", "Joystick.Madcatz_X-55");

    // open device
    if( 0 != js.open(jsDev) ) return -1;

    // loop and read joystick axis & button values
    while(1) {
        js.read(&jsv);

        printf("JS Axes (%2d) = ", jsv.dataUpdated);
        for(int i=0; i<16; i++) printf("%6.2f ", jsv.AXIS[i]);
        printf("\n");

        printf("JS Btns (%2d) = ", jsv.dataUpdated);
        for(int i=0; i<16; i++) printf("%6.2f ", jsv.BUTTON[i]);
        printf("\n");

        printf("\n");

        tm_sleep(50);
    }

    return 0;
}

